package com.ece400.abrc;

import java.io.IOException;
import java.io.OutputStream;
import java.io.PrintWriter;
import java.net.Socket;
import java.net.UnknownHostException;

import android.app.AlertDialog;
import android.util.Log;

public class RobotConnection {
	
	// Network connection variables
	
	//if on campus hostname should be 
	//your computer's net-reg id. Ex: "mjugan1laptop.nomads.utk.edu"
	public static String Hostname;
	public static int Port;
	public static boolean Connected;
	
	private static Socket robot_socket = null;
	private static OutputStream outStream;
	private static PrintWriter out;
	
	//after calibrating the robot, 0-255 is not exactly the speed scale for the left and right tracks
	//it turns out 10 is full reverse
	private static float l_full_rev = 10;
	private static float r_full_rev = 6;
	private static float l_stop = 127;
	private static float r_stop = 127;
	private static float l_full_for = 255;
	private static float r_full_for = 255;
	
	private static int x_robot;
	private static int y_robot;
	private static int x_camera;
	private static int y_camera;
	
    // Suppress default constructor for non-instantiability
    private RobotConnection() {
        throw new AssertionError();
    }
	
    public static boolean Connect(){
    	boolean success;
		
    	if(Connected == true){
    		return true;
    	}
    	
    	//set the robot and camera to not moving
    	x_robot = 127;
    	y_robot = 127;
    	x_camera = 127;
    	y_camera = 127;
    	
		//try to connect to robot's server
		success = true;
		try {
			robot_socket = new Socket(Hostname, Port);
			outStream = robot_socket.getOutputStream();
			out = new PrintWriter(outStream, true);
		}catch (UnknownHostException e) {
			success = false;
		}catch (IOException e) {
			success = false;
		}
		
		if(success == false){
			robot_socket = null;
			outStream = null;
			out = null;
		}
		
		Connected = success;
		return success;
    }
    
    public static void Disconnect(){
    	if(Connected == false){
    		return;
    	}
    	
		try {
			out.close();
			outStream.close();
			robot_socket.close();
		}
		catch (IOException e) {
			System.err.println("I/O Error for closing connection to " + Hostname);
		}
		//test
		robot_socket = null;
		outStream = null;
		out = null;
		Connected = false;
		
		return;
    }
    
    //the input arguments give a speed for the left and right tracks
    //these are number from -1 to 1
    //we translate these into the numbers for the robot
    /*
    Examples:
    	speed_l = 0, we output l_stop to the robot
    	speed_l = -1, we output l_full_rev to the robot
    	speed_r = 1, we output r_full_for to the robot
    	intermediate values are translated using a linear equation
    	So, .5 is the midpoint between stop and full_for
    */
	public static void NewMoveCmd(float speed_l, float speed_r){
    	
		String msg;
		char bytes;
    	int robot_l, robot_r;
		
		//translate -1 to 1 into full_rev to full_for
    	if(speed_l == 0){
    		robot_l = (int) l_stop;
    	}else if(speed_l > 0){
    		robot_l = Math.round((l_full_for-l_stop)*speed_l+l_stop);
    	}else{
    		robot_l = Math.round((l_stop-l_full_rev)*speed_l+l_stop);
    	}
    	if(speed_r == 0){
    		robot_r = (int) r_stop;
    	}else if(speed_r > 0){
    		robot_r = Math.round((r_full_for-r_stop)*speed_r+r_stop);
    	}else{
    		robot_r = Math.round((r_stop-r_full_rev)*speed_r+r_stop);
    	}
    	
		msg = robot_l+","+robot_r+","+x_camera+","+y_camera;
		Log.d("sending msg",msg);
		
		if(Connected == false){
			return;
		}
    	
		bytes = (char)msg.length();
		
		out.printf("%c",bytes);
		out.printf("%s",msg);
    }
    
    public static void NewDebugXY(int x, int y){
		String msg;
		char bytes;
		
    	x_robot = x;
    	y_robot = y;
    	
    	if(x_robot < 0){
    		x_robot = 0;
    	}else if(x_robot > 255){
    		x_robot = 255;
    	}    	
    	if(y_robot < 0){
    		y_robot = 0;
    	}else if(y_robot > 255){
    		y_robot = 255;
    	}
    	
		msg = x_robot+","+y_robot+","+x_camera+","+y_camera;
		Log.d("sending msg",msg);
		
		//TODO
		if(Connected == false){
			return;
		}
    	
		bytes = (char)msg.length();
		
		out.printf("%c",bytes);
		out.printf("%s",msg);
    }
}
